#include "Simbody.h"
using namespace SimTK;


/*
class PositionReporter : public PeriodicEventReporter {
public:
    PositionReporter(const MultibodySystem& system, const MobilizedBody& mobod,
                     Real reportInterval) 
    :    PeriodicEventReporter(reportInterval), system(system), mobod(mobod) {}
    void handleEvent(const State& state) const {
       system.realize(state, Stage::Position);
        Vec3 pos = mobod.getBodyOriginLocation(state);
        std::cout<<state.getTime()<<"\t"<<pos[0]<<"\t"<<pos[1]<<std::endl;
    }
private:
    const MultibodySystem&  system;
    const MobilizedBody&    mobod;
};
*/


//---------------------------------------
//Prints out the loacation and time of
//a local maximum height of a mobolized
//body
//---------------------------------------
class PositionReporter : public TriggeredEventReporter {
public:
    PositionReporter(const MultibodySystem& system, const MobilizedBody& mobod)
    :   TriggeredEventReporter(Stage::Velocity), system(system), mobod(mobod){
        getTriggerInfo().setTriggerOnRisingSignTransition(false);
    }
	Real getValue(const State& state) const {
        Vec3 vel = mobod.getBodyOriginVelocity(state);
        return vel[1]; // y coordinate
    }


	void handleEvent(const State& state) const {
        system.realize(state, Stage::Position);
        Vec3 pos = mobod.getBodyOriginLocation(state);
        std::cout<<state.getTime()<<"\t"<<pos[0]<<"\t"<<pos[1]<<std::endl;

    }
private:
	int steps;
    const MultibodySystem&  system;
    const MobilizedBody&    mobod;
};

//-------------------------------------
//Test class: alters the velocity of a
class VelocityReducer : public ScheduledEventHandler {
public:
    VelocityReducer() {
	}
	Real getNextEventTime(const State&, bool includeCurrentTime) const {
        return 25;
    }
	void handleEvent(State& state, Real accuracy, const Vector& yWeights, 
                     const Vector& ooConstraintTols, Stage& lowestModified, 
                     bool& shouldTerminate) const {
        state.updU() *= 0.2;  //reduces the velocity of state
		lowestModified = Stage::Velocity;
	
    }
};

int main() { 
    // Create the system.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
pendulumBody.addDecoration(Transform(), DecorativeSphere(0.1));
MobilizedBody::Pin pendulum1(matter.Ground(), Transform(Vec3(0)),
        pendulumBody, Transform(Vec3(0, 1, 0)));
MobilizedBody::Pin pendulum2(pendulum1, Transform(Vec3(0)),
        pendulumBody, Transform(Vec3(0, 1, 0)));
MobilizedBody::Pin pendulum3(pendulum2, Transform(Vec3(0)),
        pendulumBody, Transform(Vec3(0, 1, 0)));
//Constraint::PointOnLine(matter.Ground(), UnitVec3(0,1,0), Vec3(0), pendulum3,
//	Vec3(0));


// Set up visualization.
Visualizer viz(system);
system.addEventReporter(new Visualizer::Reporter(viz, 0.01));
system.addEventReporter(new PositionReporter(system, pendulum2));   
system.addEventHandler(new VelocityReducer());

// Initialize the system and state.
system.realizeTopology();
State state = system.getDefaultState();
pendulum2.setRate(state, 5.0);
pendulum3.setRate(state, -2.0);


// Simulate it.
RungeKuttaMersonIntegrator integ(system);
TimeStepper ts(system, integ);
ts.initialize(state);
ts.stepTo(1);
ts.stepTo(2);
ts.stepTo(5);
}